/*
	The Raja Toolkit.
	Copyright (C) 2011 Raja-Toolkit Foundation

	This library is free software; you can redistribute it and/or
	modify it under the terms of the GNU Lesser General Public
	License as published by the Free Software Foundation; either
	version 3.0 of the License, or (at your option) any later version.

	This library is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
	Lesser General Public License for more details.

	You should have received a copy of the GNU Lesser General Public
	License along with this library; if not, write to the Free Software
	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
*/

#include "queries.hpp"

namespace raja
{
namespace math
{

///
/// @name project_to_segment
/// @param seg the segment
/// @param pt the point
/// @return the projection of the point onto the segment
///
template<typename T>
T closest_on_segment(const tsegment<T>& seg, const T& pt)
{
	typedef typename T::atom_type	atom_type;

	T	seg_dir	= seg.end - seg.start;
	T	pt_dir	= pt - seg.start;


	atom_type	d_sp	= dot(seg_dir, pt_dir);
	atom_type	d_ss	= dot(seg_dir, seg_dir);	// remember, always > 0

	if( d_sp < atom_type(0) )
		return seg.start;
	else if( d_sp > d_ss )
		return seg.end;

	atom_type	t	= d_sp / d_ss; // should be float ?

	// proj	= seg.start + t * seg_dir;
	return seg[t];
}

///////////////////////////////////////////////////////////////////////////////
template vec2 closest_on_segment<vec2>(const tsegment<vec2>& seg, const vec2& pt);
template vec3 closest_on_segment<vec3>(const tsegment<vec3>& seg, const vec3& pt);

template dvec2 closest_on_segment<dvec2>(const tsegment<dvec2>& seg, const dvec2& pt);
template dvec3 closest_on_segment<dvec3>(const tsegment<dvec3>& seg, const dvec3& pt);
///////////////////////////////////////////////////////////////////////////////

///
/// distance between a point and a segment
///
template<typename T>
typename T::atom_type distance(const tsegment<T>& seg, const T& pt)
{
	typedef typename T::atom_type	atom_type;

	T	seg_dir	= seg.end - seg.start;
	T	pt_dir	= pt - seg.start;

	atom_type	d_sp	= dot(seg_dir, pt_dir);
	atom_type	d_ss	= dot(seg_dir, seg_dir);	// remember, always > 0

	if( d_sp < atom_type(0) )
		return length(pt_dir);
	else if( d_sp > d_ss )
		return length(pt - seg.end);

	atom_type	t	= d_sp / d_ss;	// should be float ?

	// proj	= seg.start + t * seg_dir;
	return length(pt - seg[t]);
}

///////////////////////////////////////////////////////////////////////////////
template real32 distance<vec2>(const tsegment<vec2>& seg, const vec2& pt);
template real32 distance<vec3>(const tsegment<vec3>& seg, const vec3& pt);

template real64 distance<dvec2>(const tsegment<dvec2>& seg, const dvec2& pt);
template real64 distance<dvec3>(const tsegment<dvec3>& seg, const dvec3& pt);
///////////////////////////////////////////////////////////////////////////////

///
/// distance between a point and a line
///
template<typename T>
typename T::atom_type distance(const tline<T>& l, const T& pt)
{
	typedef typename T::atom_type	atom_type;

	T	pt_dir	= pt - l.point;

	atom_type	d_sp	= dot(l.direction, pt_dir);
	atom_type	d_ss	= dot(l.direction, l.direction);	// remember, always > 0

	atom_type	t	= d_sp / d_ss;	// should be float ?

	// proj	= seg.start + t * seg_dir;
	return length(pt - l[t]);
}

///////////////////////////////////////////////////////////////////////////////
template real32 distance<vec2>(const tline<vec2>& l, const vec2& pt);
template real32 distance<vec3>(const tline<vec3>& l, const vec3& pt);

template real64 distance<dvec2>(const tline<dvec2>& l, const dvec2& pt);
template real64 distance<dvec3>(const tline<dvec3>& l, const dvec3& pt);
///////////////////////////////////////////////////////////////////////////////

///
/// closest to line point on a line
///
template<typename T>
T closest_on_line(const tline<T>& l, const T& pt)
{
	typedef typename T::atom_type	atom_type;

	T	pt_dir	= pt - l.point;

	atom_type	d_sp	= dot(l.direction, pt_dir);
	atom_type	d_ss	= dot(l.direction, l.direction);	// remember, always > 0

	atom_type	t	= d_sp / d_ss;	// should be float ?

	// proj	= seg.start + t * seg_dir;
	return l[t];
}

///////////////////////////////////////////////////////////////////////////////
template vec2 closest_on_line<vec2>(const tline<vec2>& l, const vec2& pt);
template vec3 closest_on_line<vec3>(const tline<vec3>& l, const vec3& pt);

template dvec2 closest_on_line<dvec2>(const tline<dvec2>& l, const dvec2& pt);
template dvec3 closest_on_line<dvec3>(const tline<dvec3>& l, const dvec3& pt);
///////////////////////////////////////////////////////////////////////////////

///
/// @brief line3 to line3 distance
/// @param l0 first line
/// @param l1 second line
/// @return distance between the two straight lines
///
template<typename T>
T distance(const tline<tvec3<T> > &l0, const tline<tvec3<T> > &l1)
{
	tvec3<T>	n	= cross(l0.direction, l1.direction);
	T			det	= dot(n, n);

#define EPSILON		(1.0 / (1024.0 * 1024.0))
	if( det < T(EPSILON) && det > -T(EPSILON) )
	{
		// lines are parallel, pick a point and compute distance
		return distance(l0, l1.point);
	}
#undef EPSILON

	T	dist = dot(l1.point - l0.point, n) / sqrt(det);

	return abs(dist);
}

///////////////////////////////////////////////////////////////////////////////
template real32 distance<real32>(const tline<vec3>& l0, const tline<vec3>& l1);
template real64 distance<real64>(const tline<dvec3>& l0, const tline<dvec3>& l1);
///////////////////////////////////////////////////////////////////////////////


///////////////////////////////////////////////////////////////////////////////

/* Ray-Triangle Intersection Test Routines          */
/* Different optimizations of my and Ben Trumbore's */
/* code from journals of graphics tools (JGT)       */
/* http://www.acm.org/jgt/                          */
/* by Tomas Moller, May 2000                        */

#define EPSILON		(1.0f / (1024.0 * 1024.0))
template <typename T>
bool intersection(const ttri3<T>& t, const tray< tvec3<T> >& r, tvec3<T>& ip)
{

	tvec3<T>	edge1, edge2, tvec, pvec, qvec;
	T			det, inv_det, u, v;

	/* find vectors for two edges sharing vert0 */
	edge1	= t.v[1] - t.v[0];
	edge2	= t.v[2] - t.v[0];

	/* begin calculating determinant - also used to calculate U parameter */
	pvec	= cross(r.direction, edge2);

	/* if determinant is near zero, ray lies in plane of triangle */
	det		= dot(edge1, pvec);

	/* calculate distance from vert0 to ray origin */
	tvec	= r.start - t.v[0];
	inv_det = T(1) / det;

	qvec	= cross(tvec, edge1);

	if( det > T(EPSILON) )
	{
		u	= dot(tvec, pvec);
		if( u < T(0) || u > det )
			return false;

		/* calculate V parameter and test bounds */
		v	= dot(r.direction, qvec);
		if( v < T(0) || u + v > det )
			return false;

	}
	else if( det < - T(EPSILON) )
	{
		/* calculate U parameter and test bounds */
		u	= dot(tvec, pvec);
		if( u > T(0) || u < det )
			return false;

		/* calculate V parameter and test bounds */
		v = dot(r.direction, qvec);
		if( v > T(0) || u + v < det )
			return false;
	}
	else return false;  /* ray is parallell to the plane of the triangle */

	T	p_t		= dot(edge2, qvec) * inv_det;

	// u	*= inv_det;
	// v	*= inv_det;

	ip	= r.start + p_t * r.direction;

	return true;
}
#undef EPSILON

///////////////////////////////////////////////////////////////////////////////
template bool intersection<real32>(const tri3 &t, const ray3 &r, vec3 &ip);
template bool intersection<real64>(const dtri3 &t, const dray3 &r, dvec3 &ip);
///////////////////////////////////////////////////////////////////////////////

///
/// @brief get the shortest segment between two straight lines
///
template<typename T>
tsegment<tvec3<T> > shortest_segment(const tline<tvec3<T> >& l0, const tline<tvec3<T> >& l1)
{
	tvec3<T>	n	= cross(l0.direction, l1.direction);
	tmat3<T>	m(l0.direction,
				  -l1.direction,
				  n);
	tmat3<T>	inv_m	= inverse(m);

	tvec3<T>	p	= inv_m * (l1.point - l0.point);

	tvec3<T>	p0	= l0[p.x];
	tvec3<T>	p1	= l1[p.y];

	return tsegment<tvec3<T> >(p0, p1);
}

///////////////////////////////////////////////////////////////////////////////
template tsegment<vec3>		shortest_segment<real32>(const line3& l0, const line3& l1);
template tsegment<dvec3>	shortest_segment<real64>(const dline3& l0, const dline3& l1);
///////////////////////////////////////////////////////////////////////////////

}	// namespace math
}	// namespace raja
